#!/usr/bin/env python
import rospy
import numpy as np
import ros_np_multiarray as ros_nm
from vision_messages.srv import GraspPoseEst, GraspPoseEstRequest
from vision_messages.msg import SingleObjPose

class GraspEstClient:
    def __init__(self):
        self.grasp_est_srv = rospy.ServiceProxy('/perception/yolograsp/grasp_est', GraspPoseEst)
        rospy.wait_for_service('/perception/yolograsp/grasp_est')
        
    def get_grasp_pose(self, req):
        # example in debug:
        # grasp_est_req = GraspPoseEstRequest()
        # grasp_est_req.single_obj_pose = SingleObjPose()
        # obj_pcs = np.zeros([1024, 3], dtype=np.float32)
        # obj_pcs = ros_nm.to_multiarray_f32(obj_pcs)
        # grasp_est_req.single_obj_pose.obj_pcs = obj_pcs
        # grasp_est_req.single_obj_pose.category_name = 'mug'
        # grasp_est_req.single_obj_pose.scale = 0.2

        try:
            return self.grasp_est_srv(req)
        except rospy.ServiceException as e:            
            rospy.loginfo("Grasp Estimation Service call failed: %s"%e)
        

if __name__ == '__main__':
    rospy.init_node('grasp_est_client')
    grasp_est_client = GraspEstClient()
    grasp_est_client.get_grasp_pose()
